package com.pid;

import android.util.Log;

import com.DataProcess.Robot;

/**
 * Created by Administrator on 2016/8/21 0021.
 */
public class PuduPID {
    private double kp = 0.0;
    private double ki = 0.0;
    private double kd = 0.0;
    private int period = 1;
    private double targetProcess = 0.0;
    private double currentProcess = 0.0;

    public int count = 0;
    public double target = 0.0;
    public double current = 0.0;
    public double err = 0.0;
    public double errSum = 0.0;
    public double errLast = 0.0;
    public double errPrevious = 0.0;
    public double pidOut;


    public PuduPID(double kp, double ki, double kd){
        setPID(kp,ki,kd);
    }

    public PuduPID(double kp, double ki, double kd, int period){
        setPID(kp,ki,kd,period);
    }

    public void cleanPID() {
        err = 0.0;
        errSum = 0.0;
        errLast = 0.0;
        errPrevious = 0.0;
        pidOut = 0.0;
        count = 0;
        targetProcess = 0.0;
        currentProcess = 0.0;
    }

    public void setPID(double kp,double ki,double kd) {
        this.kp = kp;
        this.ki = ki;
        this.kd = kd;
        Log.i("PuduPID","P:" + kp + "   I:" + ki + "   D:" + kd);
    }

    public void setPID(double kp,double ki,double kd,int period) {
        setPID(kp,ki,kd);
        if(period > 0)
            this.period = period;
    }

    public void setPeriod(int period) {
        this.period = period;
    }

    public double calculatePid(double target,double current) {

        count++;
        currentProcess += current;
        targetProcess += target;

        if (count == period) {
            this.current = currentProcess;
            this.target = targetProcess;
            Log.i("PuduPID","target:"+targetProcess+"  current:"+currentProcess);
            errPrevious = errLast;
            errLast = err;
            err = this.target - this.current;
            errSum += err;

//            positionalPID();
            incrementalPID();
            Log.i("PuduPID","pidOut"+pidOut);

            count = 0;
            targetProcess = 0.0;
            currentProcess = 0.0;
        }

        return pidOut;
    }

    //u = kp * err + ki * errSum + kd * (err - errLast)
    private void positionalPID() {

            //积分限幅度
            if (Math.abs(errSum) > 2.0) {
                errSum = 2.0 * Math.abs(errSum) / errSum;
            }
        Log.i("PuduPID","err:"+err+"errSum: " + errSum);

        pidOut = kp * err + ki * errSum + kd * (err - errLast);
    }

    //delta_u = kp * (err - errLast) + ki * err + kd * (err + errPrevious - 2 * errLast)
    private void incrementalPID() {
        //积分项限幅度
        if (Math.abs(errSum) > 2.5) {
            errSum = 2.5 * errSum / Math.abs(errSum);
            pidOut += kp * (err - errLast) + kd * (err + errPrevious - 2 * errLast);
        } else {
            pidOut += kp * (err - errLast) + ki * err + kd * (err + errPrevious - 2 * errLast);
        }

//        pidOut += kp * (err - errLast) + ki * err+ kd * (err + errPrevious - 2 * errLast);
//        Log.i("PuduPID","err:"+err+"errSum: " + errSum);

        //pidOut += kp * err + kd * (err - errLast); //更直观理解电机step by step to the target
    }
}
